import cv2
from datetime import datetime
from .cv_hough_line import detecting_lane

# cv2.putText(frame, Text, org, font, color, thickness)

def add_time_to_image(frame):
    now = datetime.now() 
    time = now.strftime("%H:%M:%S.%f")[:-3]
    font=cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(frame, "Time:"+time,
                          (200, 100), font,1,(0, 255, 255),2,cv2.LINE_4)
    return frame  

def add_servo_angle_to_image(frame,dual_axis):
    font=cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(frame, "The Angle of",
                        (300, 150), font,1,(65,105,225),2,cv2.LINE_4)
    cv2.putText(frame, "  Vertical Servo:"+str(dual_axis.vertical_servo.s.angle),
                            (300, 200), font,1,(0, 255, 255),2,cv2.LINE_4)
    cv2.putText(frame, "  Horizontal Servo:"+str(dual_axis.horizontal_servo.s.angle),
                            (300, 250), font,1,(0, 255, 255),2,cv2.LINE_4)    
    return frame  

def add_detectedlane_image(frame):
    results= detecting_lane(frame)
    #firstSquareCenters1, firstSquareCenters2,centers,laneCenters, result, mainCenterPosition= detectedlane1(frame)
    if  results is not None:
        font=cv2.FONT_HERSHEY_SIMPLEX
        maincenter=results[5]
        cv2.putText(frame, "Pos="+str(maincenter),
                    (200, 400), font,1,(0, 255, 0),2,cv2.LINE_4)
       
        car_run_text="Action: "
        car_location_text="Location: "
       
        if maincenter <= 6 and maincenter > -6:
           car_run_text=car_run_text+"Forward"
           car_location_text=car_location_text+"Middle"
        elif maincenter > 6 :
           car_run_text=car_run_text+"Turn Left"
           car_location_text=car_location_text+"Right"
        elif maincenter < -6:
           car_run_text=car_run_text+"Turn Right"
           car_location_text=car_location_text+"Left"
     
        cv2.putText(frame, car_location_text,
                    (200, 450), font,1,(0, 255, 0),2,cv2.LINE_4)
        cv2.putText(frame, car_run_text,
                    (200, 500), font,1,(0, 255, 0),2,cv2.LINE_4)
       
        x_factor=int(1920/320)
        y_factor=int(1080/240)
        firstSquareCenters1=(results[0][0]*x_factor,results[0][1]*y_factor)
        firstSquareCenters2=(results[1][0]*x_factor,results[1][1]*y_factor)
        frame=cv2.circle(frame,(int(1920/2),int(1080/2)),10,(0,0,255),-1)
        cv2.line(frame, firstSquareCenters1, firstSquareCenters2, (0, 255, 0), 1)
       
        centers=results[2]
        if centers is not None:
           centers[0]=(centers[0][0]*x_factor,centers[0][1]*y_factor)
           centers[1]=(centers[1][0]*x_factor,centers[1][1]*y_factor)
           cv2.line(frame, centers[0], centers[1], [0, 255, 0], 2)
        
        # add edge lines to the origal image
        pts1 = [[0, 1080], [1920, 1080], [1880, 30], [30, 30]]
        cv2.line(frame, (pts1[0][0],pts1[0][1]), (pts1[1][0],pts1[1][1]), (0, 255, 0), 2)
        cv2.line(frame, (pts1[1][0],pts1[1][1]), (pts1[2][0],pts1[2][1]), (0, 255, 0), 2)
        cv2.line(frame, (pts1[2][0],pts1[2][1]), (pts1[3][0],pts1[3][1]), (0, 255, 0), 2)
        cv2.line(frame, (pts1[3][0], pts1[3][1]), (pts1[0][0], pts1[0][1]), (0, 255, 0), 1)
   
        return  frame
    else:
        return  frame


def add_object_feature_image(frame):
    frame=add_time_to_image(frame)
    font=cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(frame, "Object Feature",
                            (500, 50), font,1,(0, 255, 255),2,cv2.LINE_4)
    return frame  
        
import numpy as np
def detect_red_object(frame):
      # 将BGR颜色空间转换为HSV颜色空间
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        
        # 定义红色的HSV范围
        lower_red1 = np.array([0, 120, 70])
        upper_red1 = np.array([10, 255, 255])
        lower_red2 = np.array([170, 120, 70])
        upper_red2 = np.array([180, 255, 255])
        
        # 创建红色区域的掩膜
        mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
        mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
        mask = cv2.bitwise_or(mask1, mask2)
        
        # 对掩膜进行形态学操作，去除噪声
        kernel = np.ones((5,5), np.uint8)
        mask = cv2.erode(mask, kernel, iterations=1)
        mask = cv2.dilate(mask, kernel, iterations=2)
        
        # 查找轮廓
        contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        
        # 在原图上绘制检测到的红色物体边界
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area > 500:  # 只处理面积大于500的轮廓，避免小噪声
                x, y, w, h = cv2.boundingRect(cnt)
                cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)  # 绿色边界框
                cv2.putText(frame, 'Red Object', (x, y-10), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
        return frame

def detect_rectangle(frame):
    # 转换为灰度图
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        
    # 高斯模糊降噪
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)
        
    # Canny边缘检测
    edges = cv2.Canny(blurred, 50, 150)
        
    # 查找轮廓
    contours, _ = cv2.findContours(edges.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
    # 遍历所有轮廓
    for cnt in contours:
        # 计算轮廓周长
        perimeter = cv2.arcLength(cnt, True)
           
        # 多边形近似
        approx = cv2.approxPolyDP(cnt, 0.02 * perimeter, True)
            
        # 如果是四边形(有4个顶点)
        if len(approx) == 4:
            # 计算轮廓面积
            area = cv2.contourArea(cnt)
                
            # 只处理面积大于一定值的轮廓(避免小噪声)
            if area > 1000:
                # 计算宽高比
                x, y, w, h = cv2.boundingRect(approx)
                aspect_ratio = float(w)/h
                   
                # 筛选出近似长方形的轮廓(宽高比在0.5-2之间)
                if 0.5 <= aspect_ratio <= 2.0:
                    # 在原图上绘制轮廓
                    cv2.drawContours(frame, [approx], -1, (0, 255, 0), 3)
                        
                    # 计算轮廓中心点
                    M = cv2.moments(cnt)
                    if M["m00"] != 0:
                        cX = int(M["m10"] / M["m00"])
                        cY = int(M["m01"] / M["m00"])
                            
                        # 在中心点显示"Rectangle"文字
                        cv2.putText(frame, "Rectangle", (cX - 50, cY),
                                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
        
    # 显示结果
    return frame

def detect_yellow_flower(frame):
    # 将BGR颜色空间转换为HSV颜色空间
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        
    # 定义黄色的HSV范围
    lower_yellow = np.array([20, 100, 100])
    upper_yellow = np.array([30, 255, 255])
        
    # 创建黄色区域的掩膜
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
        
    # 对掩膜进行形态学操作，去除噪声
    kernel = np.ones((5,5), np.uint8)
    mask = cv2.erode(mask, kernel, iterations=1)
    mask = cv2.dilate(mask, kernel, iterations=2)
        
    # 查找轮廓
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
    # 在原图上绘制检测到的黄色花朵
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area > 1000:  # 只处理面积大于1000的轮廓，避免小噪声
            # 绘制最小外接圆
            (x, y), radius = cv2.minEnclosingCircle(cnt)
            center = (int(x), int(y))
            radius = int(radius)
            cv2.circle(frame, center, radius, (0, 255, 255), 2)  # 黄色边界
                
            # 绘制轮廓
            cv2.drawContours(frame, [cnt], -1, (0, 255, 0), 2)  # 绿色轮廓
                
           # 添加文字标签
            cv2.putText(frame, 'Yellow Flower', (int(x)-50, int(y)-radius-10), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
        
        
    return frame  

def detect_circles(frame):
    # 转换为灰度图
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        
    # 高斯模糊降噪
    blurred = cv2.GaussianBlur(gray, (9, 9), 2)
        
    # 使用霍夫圆变换检测圆
    circles = cv2.HoughCircles(blurred, cv2.HOUGH_GRADIENT, dp=1.2, 
                                 minDist=50,
                                 param1=50, param2=30,
                                 minRadius=20, maxRadius=200)
        
    # 确保至少检测到一个圆
    if circles is not None:
        # 将圆的坐标和半径转换为整数
        circles = np.round(circles[0, :]).astype("int")
            
        # 遍历检测到的圆
        for (x, y, r) in circles:
            # 绘制圆形边界
            cv2.circle(frame, (x, y), r, (0, 255, 0), 3)
            # 绘制圆心
            cv2.circle(frame, (x, y), 2, (0, 0, 255), 3)
            # 添加文字标签
            cv2.putText(frame, f"Circle r={r}", (x - r, y - r - 10),
                           cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)
        else:
            cv2.putText(frame, "No Circles Detected", (20, 40),
                       cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        
    # 显示结果
    return frame
        
     